#include "hMath.h"
#include "SB.h"

// get normalized direction vector in XZ of the angle
Vector2 hMath::getDirectionOfAngle(float angle)
{
	if (angle < 0)
		angle += TwoPi;
	Vector2 v;
	v.x = sinf(angle);
	v.y = cosf(angle);
	return v;
}	

float hMath::getAngleOfDirection(const Vector2 &dir)
{
    float fAngle = atan2f(dir.y, dir.x);
    fAngle = (fAngle > 0.0 ? fAngle : (TwoPi + fAngle)); // put the angle in from 0 to 2Pi format
    return fAngle;
}

float hMath::blend(float _fFrom, float _fTo, float _fSpeed)
{
    if (_fFrom == _fTo) return _fFrom;
    
    if (_fFrom < _fTo)
    {
        _fFrom += _fSpeed * SB::dt;
        if (_fFrom > _fTo) return _fTo;
        return _fFrom;
    }
    if (_fFrom > _fTo)
    {
        _fFrom -= _fSpeed * SB::dt;
        if (_fFrom < _fTo) return _fTo;
        return _fFrom;
    }
    return _fTo;
}

Vector4 hMath::blend(Vector4 _fFrom, Vector4 _fTo, float _fSpeed)
{
    return Vector4(
        blend(_fFrom.x, _fTo.x, _fSpeed),
        blend(_fFrom.y, _fTo.y, _fSpeed), 
        blend(_fFrom.z, _fTo.z, _fSpeed),
        blend(_fFrom.w, _fTo.w, _fSpeed));
}